#include <Servo.h>
//导入舵机库 需要安装MsTimer2库


#Pin_YAW  8; //定义偏转舵机引脚
#Pin_PITCH 9; //定义俯仰舵机引脚
#Pin_PITCH_read  10;//读取pitch旋钮值的引脚
#Pin_YAW_read    11;//读取yaw旋钮值的引脚

int time_yaw=20;//Ms
int time_pitch = 30;//Ms
float theta_yaw=0;
float theta_pitch=0;

Servo servo_YAW;
Servo servo_PITCH;
//实例两个舵机

void yaw_handler()
{
    theta_yaw= analogRead(Pin_YAW_read);
    map(theta_yaw,0,255,-90,90);
    yaw(theta_yaw);
}

void pitch_handler()
{
    theta_pitch = analogRead(Pin_PITCH_read);
    map(theta_yaw,0,255,-90,90);
    pitch(theta_pitch);
}

void yaw(float theta=0.0)
{
    servo_YAW.write(theta);
    //绕z轴偏转theta°
}

void pitch(float theta=0.0)
{
    servo_PITCH.write(theta);
    //绕y轴俯仰theta°
}

void set_up()
{
    servo_YAW.attach(Pin_YAW);
    servo_PITCH.attach(Pin_PITCH);//初始化舵机连接
    
    //设置引脚输入模式
    pinMode(Pin_PITCH_read,INPUT);
    pinMode(Pin_YAW_read,INPUT);
}


void loop()
{
    pitch_handler();
    delayMicroseconds(time_pitch);//间隔扫描 
    yaw_handler();
    delayMicroseconds(time_yaw);
}

